/* exponencial smoothing */
#include <stdio.h>
#include <stdint.h>
#include <stdlib.h>
#include "ksququ.h"

typedef struct
{
    int16_t  s;
    int16_t  s_prev;
    int32_t  n;
    int32_t  y_prev;
    int32_t  y_sum;
}sfilter;

#define min(a,b)     (((a)<(b))?(a):(b))
#define max(a,b)     (((a)>(b))?(a):(b))

static int32_t __floorlog2(uint32_t n) {
    int32_t pos = 0;
    if (0 == n) {
        return -1;
    }

    if (n >= 0x10000 ) { /* 0x10000 = 1 << 16 */
        n = n >> 16; 
        pos += 16; 
    }
    if (n >= (1 << 8) ) { 
        n >>=  8; 
        pos +=  8; 
    }
    if (n >= 1<< 4) { 
        n >>=  4; 
        pos +=  4; 
    }
    if (n >= 1<< 2) {
        n >>=  2;
        pos +=  2; 
    }
    if (n >= 1<< 1) {
        pos +=  1; 
    }
    return pos;
}

static int32_t _order(int32_t n) {
    if (n < 0) {
        n = -n;
    }
    else if (0 == n) {
        return 0;
    }else {
    /* nothing to do */
    } 
    return __floorlog2((uint32_t)n);
}

/* 曲线滤波 */
void onelv_smooth(int32_t* nsec_current, sfilter* filt)
{
    int32_t s = 0, s2 = 0;
    filt->n++; /* increment number of samples */
    if (1 == filt->n) {
    	filt->y_prev = *nsec_current;
    	filt->y_sum = *nsec_current;
    	filt->s_prev = 0;
    }
    s = filt->s;
    /* speedup filter, if not 2^s > n */
    if ( (1 << s) > filt->n ) {
	/* lower the filter _order */
		s = _order(filt->n);
	} 
	else {
		/* avoid overflowing of n */
		filt->n = 1<<s;
	}
    /* avoid overflowing of filter. 30 is because using signed 32bit integers */
    s2 = 30 - _order(max(filt->y_prev, *nsec_current));
    /* use the lower filter order, higher will overflow */
    s = min(s, s2);
    /* if the order of the filter changed, change also y_sum value */
    if(filt->s_prev > s) {
		filt->y_sum = filt->y_sum >> (filt->s_prev - s);
	}else if (filt->s_prev < s) {
		filt->y_sum = filt->y_sum << (s - filt->s_prev);
	}else {
    /* nothing to do */
    } 
    /* compute the filter itself */
    filt->y_sum += *nsec_current - filt->y_prev;
    filt->y_prev = filt->y_sum >> s;
    /* save previous order of the filter */
    filt->s_prev = (int16_t)s;
    /* dbg("filter: %d -> %d (%d)\n", *nsec_current, filt->y_prev, s); */
    /* actualize target value */
    *nsec_current = filt->y_prev;
}

#define FILTER_A 0.03F
int frist_order(int input) {
    static int val = 0;
    val = (int)( (float)input * FILTER_A + (1.0F - FILTER_A) * (float)val);
    return val;
}


#define KSMOOTH_COMM5(input) ({\
    static sfilter a_filt = {5, 0, 0, 0, 0};\
    int32_t data = input;\
    onelv_smooth(&data, &a_filt);\
    data;\
})\

#define KSMOOTH_COMM4(input) ({\
    static sfilter a_filt = {4, 0, 0, 0, 0};\
    int32_t data = input;\
    onelv_smooth(&data, &a_filt);\
    data;\
})\

#define KSMOOTH_COMM3(input) ({\
    static sfilter a_filt = {3, 0, 0, 0, 0};\
    int32_t data = input;\
    onelv_smooth(&data, &a_filt);\
    data;\
})\

#define KSMOOTH_COMM(n, input) ({\
        static sfilter a_filt = {n, 0, 0, 0, 0};\
        int32_t data = input;\
        onelv_smooth(&data, &a_filt);\
        data;\
    })\


#define QMAXSIZE 150
KSQELEM qbuf1[QMAXSIZE] = {0};
KSQELEM qbuf2[QMAXSIZE] = {0};
KSQELEM qbuf3[QMAXSIZE] = {0};

struct s_ksqueue ksq1 = { .idx = 0, .itmes = 0, .depth = QMAXSIZE, .data = qbuf1 };
struct s_ksqueue ksq2 = { .idx = 0, .itmes = 0, .depth = QMAXSIZE, .data = qbuf2 };
struct s_ksqueue ksq3 = { .idx = 0, .itmes = 0, .depth = QMAXSIZE, .data = qbuf3 };

#define Q1_QUEUE_PUT(pe) ksqueue_in(&ksq1, pe)
#define Q1_QUEUE_CLEAR() ksqueue_clear(&ksq1)
#define Q1_QUEUE_SD() ksqueue_visit_sd(&ksq1)

#define Q2_QUEUE_PUT(pe) ksqueue_in(&ksq2, pe)
#define Q2_QUEUE_CLEAR() ksqueue_clear(&ksq2)
#define Q2_QUEUE_SD() ksqueue_visit_sd(&ksq2)

#define Q3_QUEUE_PUT(pe) ksqueue_in(&ksq3, pe)
#define Q3_QUEUE_CLEAR() ksqueue_clear(&ksq3)
#define Q3_QUEUE_SD() ksqueue_visit_sd(&ksq3)

int buf[300] = {0};
int buf0[300] = {0};

int main(void) {
    int input = 0, output = 0,  o2 = 0;
    int i = 0;
    for(i = 0; i < 300; i++) {
        input = rand()%5000 + 10000 + i*100;
        output = KSMOOTH_COMM(5, input);
        buf0[i] = input;
        buf[i] = output;
        o2 = frist_order(input);
       // printf("%04d\r\n", o2);
        Q1_QUEUE_PUT(input);
        Q2_QUEUE_PUT(output);
        Q3_QUEUE_PUT(o2);
    }
 //   printf("\r\n\r\n");

//    for(i = 0; i < 300; i++) { 
//        printf("%04d\r\n", buf[i]);
//    }
//    printf("\r\n\r\n");
//    for(i = 0; i < 300; i++) { 
//        printf("%04d\r\n", buf0[i]);
//    }
      
    printf("\r\n");
//    printf("output is %d\r\n", output);
//    printf("output is %d\r\n", o2);
   printf("sd is %d / %d / %d\r\n", Q1_QUEUE_SD(), Q2_QUEUE_SD(), Q3_QUEUE_SD() );
}





